#ifndef MYMPC_H
#define MYMPC_H

#include <vector>
#include <map>
#include <Eigen/Core>

using namespace std;

class MPC
{
    public:
        MPC();
    
        // Solve the model given an initial state and user' command.
        // Return the first actuatotions.
        vector<double> Solve(Eigen::VectorXd state, const double cmd_w);
        // predicted value
        vector<double> mpc_x;
        vector<double> mpc_y;
        vector<double> mpc_theta;

        double _mpc_totalcost;

        void LoadParams(const std::map<string, double> &params);
    
    private:
        // Parameters for mpc solver
        double _max_angvel, _max_speed, _bound_value;
        int _mpc_steps, _x_start, _y_start, _theta_start, _distance_start, _degree_start, _angvel_start;
        std::map<string, double> _params;

        unsigned int dis_cnt;
};

#endif /* MYMPC_H */